
library IEEE;
use IEEE.std_logic_1164.all;
use work.state_pkg.all;

entity cross_state_ctrl is
	port (	clk			: in	std_logic;
		reset			: in	std_logic;

		sensor_l		: in	std_logic;
		sensor_m		: in	std_logic;
		sensor_r		: in	std_logic;
		R         : in std_logic;
		L         : in std_logic;

		
		continu : out std_logic;
		rover_direction : out direction_type;
		
		dbm : out string (2 downto 1)
	);
end entity cross_state_ctrl;



architecture behavioural of cross_state_ctrl is
   type cross_state is (RESET_STATE, STRAIGHT_STATE, RIGHT_STATE, LEFT_STATE, KEEP_TURN_RIGHT_STATE, KEEP_TURN_LEFT_STATE, DONE_STATE);
   signal state, newstate: cross_state;
   
    signal sensor: std_logic_vector(2 downto 0);   
    signal continu_state : std_logic;
    
    begin
    sensor <= sensor_l & sensor_m & sensor_r;
    
    process (clk)
      begin
        if (rising_edge (clk)) then
            if (reset = '1') then
                state <= RESET_STATE;
            else
                state <= newstate;
            end if;
        end if;
    end process;
    
  lblstate: process(state, reset, sensor, sensor_l, sensor_m, sensor_r)
  begin
  case state is 
    when RESET_STATE =>
      rover_direction <= STRAIGHT;
		continu <= '1';
      dbm <= "RS";
      if (sensor = "000" AND L = '0' AND R = '0') then
        newstate <= STRAIGHT_STATE;
      elsif (sensor = "000" AND L = '1' and R = '0') then 
        newstate <= LEFT_STATE;
      elsif (sensor = "000" AND R = '1' and L = '0') then
        newstate <= RIGHT_STATE;
		else
		  newstate <= RESET_STATE;
      end if;
		
    when STRAIGHT_STATE =>
      rover_direction <= STRAIGHT;
      continu <= '1';
      dbm <= "SS";
      if (sensor = "000") then
        newstate <= STRAIGHT_STATE;
      else
        newstate <= DONE_STATE;
      end if;
		
    when LEFT_STATE =>
      rover_direction <= SOFT_LEFT;
      continu <= '1';
      dbm <= "tL";
      if (sensor_m = '1') then --sensor_r might be better sensor_m...
        newstate <= KEEP_TURN_LEFT_STATE;
      else
        newstate <= LEFT_STATE;
      end if;
		
    when KEEP_TURN_LEFT_STATE =>
      rover_direction <= SOFT_LEFT;
      continu <= '1';
      dbm <= "kL";
      if (sensor = "101") then --could also test for "001"
        newstate <= DONE_STATE;
      else
        newstate <= KEEP_TURN_LEFT_STATE;
      end if;
		
    when RIGHT_STATE =>
      rover_direction <= SOFT_RIGHT;
      continu <= '1';
      dbm <= "tR";
      if (sensor_m = '1') then
        newstate <= KEEP_TURN_RIGHT_STATE;
      else
        newstate <= RIGHT_STATE;
      end if;
		
    when KEEP_TURN_RIGHT_STATE =>
      rover_direction <= SOFT_RIGHT;
      continu <= '1';
      dbm <= "kR";
      if (sensor= "101") then
        newstate <= DONE_STATE;
      else
        newstate <= KEEP_TURN_RIGHT_STATE;
      end if;
		
    when DONE_STATE =>
      rover_direction <= STRAIGHT;
		continu <= '0';
      dbm <= "dS";
      newstate <= DONE_STATE;
		
  end case;
  end process;
end architecture behavioural;
    